using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace PathFinding
{
    public class PFAgent : MonoBehaviour
    {
        public PFGrid pFGrid;
        public float Speed = 5f;
        public float Rot = 0.3f;
        public float Acc = 2f;
        public float StopAcc = 4f;

        public Transform Goal;
        public float StartPathFindingThreshold = 2;
        public float ReachPointThreshold = 0.5f;

        public Vector3 lastGoalPos;

        public Vector3[] path;
        public int nextPathPointIndex = 0;
        public Vector3 currentPathTarget;
        public Vector3? nextPathTarget = null;

        bool canMove = true;

        float currentSpeed = 0;
        public float backwardTime = 0;
        public float MaxBackwardTime = 2;
        public float MaxBackwardTimeCooldown = 3;

        private void Awake()
        {
            if (pFGrid == null)
                throw new Exception("No pFGrid");
            pFGrid.SelfUpdate = false;
        }

        // Start is called before the first frame update
        void Start()
        {
            if (Goal == null)
                throw new Exception("No Goal");
        }

        void SpeedForward()
        {
            if (currentSpeed > 0)
                currentSpeed = Mathf.Clamp(currentSpeed + Acc * Time.deltaTime, -Speed, Speed);
            else
                currentSpeed = Mathf.Clamp(currentSpeed + StopAcc * Time.deltaTime, -Speed, Speed);
        }

        void SpeedBackward()
        {
            if (currentSpeed > 0)
            {
                currentSpeed = Mathf.Clamp(currentSpeed - StopAcc * Time.deltaTime, -Speed, Speed);
            }
            else
            {
                currentSpeed = Mathf.Clamp(currentSpeed - Acc * Time.deltaTime, -Speed, Speed);
            }
        }

        void SlowDown()
        {
            if (currentSpeed > 0)
                currentSpeed = Mathf.Clamp(currentSpeed - StopAcc * Time.deltaTime, 0, Speed);
            else if (currentSpeed < 0)
                currentSpeed = Mathf.Clamp(currentSpeed + StopAcc * Time.deltaTime, -Speed, 0);
        }

        // Update is called once per frame
        void Update()
        {
            if (Goal != null &&
                (Goal.position - lastGoalPos).magnitude > StartPathFindingThreshold &&
                (Goal.position - transform.position).magnitude > StartPathFindingThreshold)
            {
                lastGoalPos = Goal.position;
                lastGoalPos.y = 0;
                if (pFGrid.CallAPath(transform.position, lastGoalPos, ref path))
                {
                    nextPathPointIndex = 0;
                    currentPathTarget = path[nextPathPointIndex];
                    if (nextPathPointIndex + 1 < path.Length)
                    {
                        nextPathTarget = path[nextPathPointIndex + 1];
                    }
                    else
                        nextPathTarget = null;
                }
                else
                {
                    Debug.Log("Can't find path from " + transform.position + " to " + lastGoalPos);
                }
            }



            if (canMove && (currentPathTarget - transform.position).magnitude > ReachPointThreshold)
            {
                // keep moving to currentPathTarget
                var dir = currentPathTarget - transform.position;
                var dot = Vector3.Dot(transform.forward, dir);

                dir = dir.normalized;
                //var angle = Mathf.Clamp(Mathf.Acos(dot), 0, Rot * Time.deltaTime);
                //transform.Rotate(Vector3.up, Mathf.Atan2(dir.z, dir.x));
                var nextPos = transform.position + transform.forward * currentSpeed * (currentSpeed / StopAcc);
                var dot2 = Vector3.Dot(transform.forward, currentPathTarget - nextPos);

                if (dot > 0 || backwardTime > MaxBackwardTime)
                {
                    transform.rotation = Quaternion.RotateTowards(transform.rotation, transform.rotation * Quaternion.FromToRotation(transform.forward, dir), Rot * Time.deltaTime);
                    backwardTime = Mathf.Max(backwardTime - Time.deltaTime, 0);
                    if (dot2 > 0)
                    {
                        SpeedForward();
                    }
                    else if(dot2 != 0)
                    {
                        SlowDown();
                    }
                }
                else if (dot != 0)
                {
                    transform.rotation = Quaternion.RotateTowards(transform.rotation, transform.rotation * Quaternion.FromToRotation(transform.forward, -dir), Rot * Time.deltaTime);
                    if (dot2 < 0)
                    {
                        SpeedBackward();
                    }
                    else if (dot2 != 0)
                    {
                        SlowDown();
                    }

                    backwardTime += Time.deltaTime;

                    if (backwardTime > MaxBackwardTime)
                    {
                        backwardTime = MaxBackwardTime + MaxBackwardTimeCooldown;
                    }
                }

            }
            else
            {

                if (path != null && nextPathPointIndex + 1 < path.Length)
                {
                    ++nextPathPointIndex;
                    currentPathTarget = path[nextPathPointIndex];
                    if (nextPathPointIndex + 1 < path.Length)
                    {
                        nextPathTarget = path[nextPathPointIndex + 1];
                    }
                    else
                        nextPathTarget = null;
                }
                else
                {// 减速
                    SlowDown();
                    backwardTime = 0;
                }
            }

            if (!canMove)
            {
                SlowDown();
            }

            transform.position += transform.forward * currentSpeed * Time.deltaTime;
        }



        private void OnDrawGizmos()
        {
            if (path != null && path.Length > 0)
            {
                foreach (var p in path)
                {
                    Debug.DrawLine(p, p + Vector3.up * 2, Color.blue);

                }
            }

            Debug.DrawLine(transform.position, currentPathTarget, Color.cyan);
            Debug.DrawLine(currentPathTarget, currentPathTarget + Vector3.up * 2, Color.green);

        }


    }



}
